// Serial MCU control
// Mario Ninic
// 04/21/2020

// motor control registers
int EN1 = 8;  //M1
int EN2 = 11; //M1
int DIR1 = 9; //M2
int DIR2 = 12;//M2
//MCU Commands
int Stop = 0;
int Fwd = 1;
int Bwd = 2;
int Left = 3;
int Right = 4;
int MtrCmdDelay = 100;  // motor command delay ms
int PostMtrCmdDelay = 100;  // motor command delay ms
char incomingByte = 0;  // for incoming serial data
char message[9];        // reserve space for 9 bytes
// 8 bytes.
//[0] Motor Direction;
//[1,2,3] Future use;
//[4,5,6] Future use;
//[7] termination chr
void setup(void) 
{
  pinMode(EN1, OUTPUT);     // sets the digital pin 8 as output
  pinMode(DIR1, OUTPUT);    // sets the digital pin 9 as output
  pinMode(EN2, OUTPUT);     // sets the digital pin 11 as output
  pinMode(DIR2, OUTPUT);    // sets the digital pin 12 as output
  MCU(Stop);  // stop the motor 
  Serial.setTimeout(500); // set serial timeout (ms)
  delay(200);
  Serial.begin(57600);
  //Serial.println("Motor Control Demo. Mario Ninic");
}

void loop(void) //Main loop
{
 // serial commands 
  if (Serial.available() > 0) {
      //read the incoming byte:
      Serial.readBytes(message, 4);  // read buffer
      MCU (message[0] - '0'); // call MCU function and pass direction parameter
      //delay(10);
     }
  else{
     MCU (Stop); //STOP
     delay(10);
     } 
     //Serial.println("End loop");
  }

//function MCU

void MCU(int Motion){

switch (Motion){
  case 0:     //stop motor
    digitalWrite(EN1, LOW);  // disable motor1
    digitalWrite(EN2, LOW);  // disable motor2
  break;
  
  case 1:     //set direction and move fw
    digitalWrite(DIR1, HIGH);  // set M1 direction
    digitalWrite(DIR2, LOW);   // set M2 direction
    delay(MtrCmdDelay);
    digitalWrite(EN1, HIGH);   // enable M1
    digitalWrite(EN2, HIGH);   // enable M2
    delay (PostMtrCmdDelay);
  break;
  
  case 2:    //move backward
    digitalWrite(DIR1, LOW);   // set M1 direction
    digitalWrite(DIR2, HIGH);  // set M2 direction
    delay(MtrCmdDelay);
    digitalWrite(EN1, HIGH);  // enable M1
    digitalWrite(EN2, HIGH);  // enable M2
    delay (PostMtrCmdDelay);
  break;

  case 3:      //move left
    digitalWrite(DIR1, HIGH);  // set M1 direction
    digitalWrite(DIR2, HIGH);  // set M2 direction
    delay(MtrCmdDelay);
    digitalWrite(EN1, HIGH);   // enable M1
    digitalWrite(EN2, HIGH);   // enable M2
    delay (PostMtrCmdDelay);
  break;

  case 4:     //move right
    digitalWrite(DIR1, LOW);   // set M1 direction
    digitalWrite(DIR2, LOW);   // set M2 direction
    delay(MtrCmdDelay);
    digitalWrite(EN1, HIGH);  // enable M1
    digitalWrite(EN2, HIGH);  // enable M2
    delay (PostMtrCmdDelay);
  break;
  default:     //stop motor
   delay(10); // do nothing
   //digitalWrite(EN1, LOW);  // disable motor1
   // digitalWrite(EN2, LOW);  // disable motor2
  break;
    }
  }